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CLAIMS 

1. A system for estimating the position, speed and orientation of a vehicle (10), 
comprising: 

means for determining the components of two noncollinear constant unit vectors 
g b ,e b according to vehicle body axes; 

means for determining the components of said noncollinear constant unit 
vectors g n e t according to the Earth's axes; and 

means for determining the three components of the angular velocity 5^ of the 
vehicle in body axes. 

characterized in that the system comprises 

means for correcting said angular velocity 8^ with a correction u w and 

obtaining a corrected angular velocity a> b = + ; 

a module for integrating the kinematic equations (13) of the vehicle receiving 
the corrected angular velocity & b as input and providing the transformation matrix B for 
transforming Earth's axes into vehicle body axes and the orientation of the vehicle in 

the form of Euler angles O ; 

a synthesis module (15) of the components in body axes of the two noncollinear 
constant unit vectors to provide an estimation of said noncollinear vectors in body axes 
g b ,e b , where said estimation is calculated as follows: 

S b =Bg t 
e b =Be t 

a control module (14) implementing a control law to calculate said correction u a , 
where said control law is: 

w fi ,=cr(| fe xg i +^x^) [1] 
where a is a positive scalar, 

such that by applying this correction to the measured angular velocity cb b and using 
the resulting angular velocity 0% = + u w as input to the module for integrating the 
kinematic equations, the latter are stable in the ISS sense and the error in the 

estimation of the direction cosine matrix B and of the Euler angles 4> is bounded. 

2. A system according to claim 1, characterized in that said noncollinear unit 
vectors g,e are local gravity g and projection of the magnetic field on the plane 
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perpendicular to gravity e . 

3. A system according to claim 2, characterized in that the means for determining 
the components of the two noncollinear constant unit vectors g b ,e b according to 
vehicle body axes include: 

5 - an Inertial Measurement Unit (IMU) (21) including a group of at least three 
gyroscopes (211) and at least three accelerometers (212) located along the vehicle 
body axes; 

a magnetometer (22) able to measure the Earth's magnetic field according to 
the vehicle body axes; 
10 - static pressure (231) and differential pressure (232) sensors; 

two vanes (24) to measure the angles of attack (241) and side slip. (242); 
an angular velocity acquisition and processing module (11) configured to 
acquire the angular velocity & b (t) and delay it to obtain & b (t-r) ; 

a data acquisition and processing module (17) configured to acquire the specific 
15 force a b (t) measured by the accelerometers (212), the static pressure p s (t) 
measured in sensor (231), the differential pressure p d (t) measured in sensor (232), 
the angle of attack a(t) measured in sensor (241), the angle of sideslip /3(t) 
measured in sensor (242) and the value of the Earth's magnetic field m b (t) measured 
in the magnetometer (22), delay them and process them to calculate the true airspeed 
20 v(r - r) , the airspeed in body axes v b (t - r) as follows: 

v cos a cos p 
v h - Osin p 
C sin a cos /? 

the numerical derivative of the airspeed in body axes Vb(t — r) , 
the local gravity in body axes g b as follows: 

g b {t-r)^^ b {t-T) + & b {t-T)^v b {t~T)-a b {t~r) 

25 and the projection of the Earth's magnetic field on the horizontal plane perpendicular to 
local gravity e{t-r) as follows: 

e b (t-r) = fh b {t - r) - m b (t - r) ■ = % . 

M'-*)| 

4. A system according to claim 3, characterized in that the means for knowing the 
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components of the two noncollinear constant unit vectors g n e t according to Earth's 

axes include: 

a GPS (Global Positioning System) receiver (25); 
and in that the data provided by the GPS are acquired, processed and used in said 
5 data acquisition and processing module (17) to calculate the components of the two 
noncollinear constant unit vectors g n e t according to the Earth's axes. 

5. A system according to claim 3, characterized in that the system includes a 
Savitzky-Golay filter (179) where v b , numerical derivative of v b , is calculated. 

6. A system according to claim 1 including: 

10 - means of acquiring data from a group of sensors (25, 231) located in the 
. vehicle, providing position and speed according to Earth's axes P n V t ; 

means of acquiring data from another group of sensors (212) Ideated in the 
vehicle, providing specific force a b in body axes; 

a navigation module where the navigation equations of the vehicle are 
15 integrated from the specific force a b and the direction cosine matrix B to obtain 

calculated position and speed in local axes and corrected in a Kalman filter to obtain 
estimated position and speed in local axes. 

7. A method for estimating the position, speed and orientation of a vehicle (10) 
comprising: 

20 - calculating the components of two noncollinear constant unit vectors g b ,e b 

according to vehicle body axes from measurements of sensors located in the vehicle 
according to the body axes of the latter; 

calculating the components of said noncollinear constant unit vectors g n e t , 

according to the Earth's axes from measurements of sensors located in the vehicle 
25 which provide position in local axes; 

measuring the three components of angular velocity & b of the vehicle in body 

axes; 

correcting the angular velocity S> b with a correction u m and obtaining a 
corrected angular velocity cb b = & b + u a ; 
30 - integrating the kinematic equations of the vehicle, according to the corrected 
angular velocity 6) b and providing the transformation matrix B for transforming the 
Earth's axes into vehicle body axes and the orientation of the vehicle in the form of 
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Euler angles O ; 

calculating an estimation of the components in body axes of the two 
noncollinear constant unit vectors g b ,e b , where said estimation is calculated as follows: 

S b = Bg t 
e b = Be, 

5 - obtaining the correction u a by means of the control law: 

u m =a{g b xg b + e k xe b ) [1] 
where a is a positive scalar, 

such that upon applying this correction to the measured angular velocity 6> b and 
using the resulting angular velocity d) b ^fy+u^ as input to the module for integrating 
10 the kinematic equations, the latter are stable in the ISS sense and the error in the 

estimation of the direction cosine matrix B and of the Euler angles 6 is bounded. 
8. A method according to claim 7, characterized in that said noncollinear unit 
vectors g,e are local gravity g and projection of the magnetic field on the plane 
perpendicular to gravity e . 
15 9. A method according to claim 8, characterized in that it comprises: 

measuring specific force a b (t) , static pressure p s (t) , differential pressure 

p d (t) , angle of attack a(j) , angle of sideslip fi(J) and the value of the Earth's 
magnetic field m b (t); 

calculating the true airspeed v(t) from the differential pressure p d (t) and static 
20 pressure p s (t) measurements and from knowing the outside temperature at the initial 
moment T 0 ; 

calculating the airspeed in body axes as follows: 

v cos a cos /3 

Csin p 
C sin a cos /? 

delaying a time r the angular velocity & b (J) , specific force a b (t) , magnetic 
25 field m b (t) and airspeed in body axes v b (t) ; 

calculating the numerical derivative of the airspeed in body axes v b (t-r) ; 
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calculating the local gravity in body axes g b as follows: 

g b (t-T) = v b (t-T) + & b (t-r)xv b (t-T)-a b (t-T)]y, 

calculating the projection of the Earth's magnetic field on the horizontal plane 
perpendicular to local gravity as follows: 

\g h (t-T)\ 



10. A method according to claim 9, characterized in that the position in Earth axes 
P t is measured and from this measurement the components of the two noncollinear 
constant unit vectors g n e t are calculated according to the Earth's axes. 

11. A method according to any of claims 9-10, characterized in that v b , the 
10 numerical derivative of v b , is calculated in a Savitzky-Golay filter (179). 

12. A method according to any of claims 7-1 1 including: 
measuring position and speed in Earth-fixed axes P n V t \ 
measuring specific force a b in body axes; 

integrating the navigation equations of the vehicle according to the specific 

15 force a b and the direction cosine matrix B to obtain the calculated position and 

speed in local axes and they are corrected in a Kalman filter to obtain estimated 
position and speed in local axes. 
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